#include <ros/ros.h>
 
#include <skeleton_listener/Coord.h>
#include <skeleton_listener/Gripper.h>
#include <body_msgs/Skeletons.h>
#include <body_msgs/Hands.h>
#include <math.h>
 
using namespace std;

ros::Publisher pubSkel;
ros::Publisher pubGrip;

int openG, closedG = 0;
float gC[3]={0};
float littleDelta[3]={0};

/***********************************************
* Callback for hand state, later used by Grip()
************************************************/
void handsCallback(body_msgs::Hands handy)
{
	skeleton_listener::Gripper Grip;

        int right = 0;
        if (handy.hands[0].left==true)
            right = 0;
        else if(handy.hands[1].left==true)
            right = 1;
 
        char hand_config[10];
        strcpy(hand_config, (handy.hands[right].state).c_str());
        if(strcmp(hand_config,"closed"))
        {
            openG = 0;
            closedG++;
        }
        else
        {
            closedG = 0;
            openG++;
        }
	if(closedG == 3){
		Grip.grip = false;
		pubGrip.publish(Grip);}
	else if(openG == 3){
		Grip.grip = true;
		pubGrip.publish(Grip);}
}

/**************************************************
* Callback for updating the positions of the body
***************************************************/
void skeletonsCallback(body_msgs::Skeletons skel_array)
{ 
    skeleton_listener::Coord c;

    c.rx = skel_array.skeletons[0].right_hand.position.x;
    c.ry = skel_array.skeletons[0].right_hand.position.y;
    c.rz = skel_array.skeletons[0].right_hand.position.z;
 
    c.lx = skel_array.skeletons[0].left_hand.position.x;
    c.ly = skel_array.skeletons[0].left_hand.position.y;
    c.lz = skel_array.skeletons[0].left_hand.position.z;

    c.lsz = skel_array.skeletons[0].left_shoulder.position.z;

	float norm = sqrt(pow(c.rx-gC[0],2)+pow(c.ry-gC[1],2)+pow(c.rz-gC[2],2));

	if(norm>0.01)
	{
	   cout << "Stor rörelse:" <<norm << endl;
	   pubSkel.publish(c);
	}
	if(norm<0.01 && norm>0.003)
	{
	   cout << "Medelstor rörelse:" <<norm << endl;
	   pubSkel.publish(c);
	}
	if(norm<0.003)
	{
	   cout << "Liten rörelse:" <<norm << endl;
	   if(0.005<sqrt(pow(c.rx-littleDelta[0],2)+pow(c.ry-littleDelta[1],2)+pow(c.rz-littleDelta[2],2)))
	   {
		cout << "\nMoved\n" << endl;
		pubSkel.publish(c);
		   littleDelta[0]=c.rx;
		   littleDelta[1]=c.ry;
		   littleDelta[2]=c.rz;
	   }	
	}

	gC[0]=c.rx; gC[1]=c.ry; gC[2]=c.rz; 
}


//--Main-----------------------
int main(int argc, char **argv)
{
    ros::init(argc, argv, "kinectSubPub_experimental");
    ros::NodeHandle n;
    ros::Subscriber subS = n.subscribe("skeletons", 1000, skeletonsCallback);
    ros::Subscriber subH = n.subscribe("hands_pros", 1000, handsCallback);
    pubSkel = n.advertise<skeleton_listener::Coord>("Coord", 1000);
    pubGrip = n.advertise<skeleton_listener::Gripper>("Gripper", 1000);

    //if(system("gnome-terminal --command \"/opt/ros/diamondback/ros/bin/rosrun skeleton_listener Control_robot 130.243.85.167 1025\"")){};
    
    ros::spin();
 
    return 0;
 
}
